"""
飞行历史记录插值器

date: 2025/8/21
author: SiHeng Tang
file: history_interpolation.py
copyright(c) DFSA Eastern Software
此程序不能提供任何担保 WITHOUT WARRANTY OF ANY KIND
"""
import logging

from mj_errors import MJRuntimeError
from task_element import DataPack

logger = logging.getLogger(__name__)


class DistantHistoryError(MJRuntimeError):
    """
    遥远的历史，产生的原因有两个，请求的时间直接超出了历史记录的显性要求时长，
    或者由于系统时间校准等原因，历史缓存不能提供有效的回溯支持，无法计算历史坐标。
    """
    pass


class LinerInterpolation:
    """
    线性插值器，同时示范插值器写法
    """

    def __init__(self, max_trace_time):
        # 此处
        self.max_trace_time = max_trace_time
        self.last_update_time_e7 = 0
        self._h_list = {}

    def update_frame(self, dp: DataPack):
        if not bool(dp.get_a("payload")):
            return

        # 飞行历史使用 1E7 时间
        # 新历史入栈，处理超时历史
        this_ptime_e7 = dp_ptime_e7 = int(dp.get_a("pack_time") * 1e7)
        self._h_list.setdefault(dp_ptime_e7, dp.get_a("payload"))

        out_date_keys = []
        for history_ptime in self._h_list.keys():
            if abs(history_ptime - this_ptime_e7) > int(self.max_trace_time * 1e7):
                out_date_keys.append(history_ptime)

        for k in out_date_keys:
            self._h_list.pop(k)

        self.last_update_time_e7 = this_ptime_e7
        logger.debug(f"update interpolation at {this_ptime_e7=} history length: {len(self._h_list)}")

    def get_history_at(self, trace_sec: float) -> DataPack:
        # 线性插值实现历史访问
        # 线性插值器首先找出两个最近的时间点，然后使用线性函数生成最终的值
        target_abs_time_e7 = self.last_update_time_e7 - int(trace_sec * 1e7)
        down_limit_e7, up_limit_e7 = 0, self.last_update_time_e7
        # down_limit < target_abd_time_e7 < up_limit_e7
        for history_time_e7 in self._h_list.keys():
            if (history_time_e7 < target_abs_time_e7) and (history_time_e7 > down_limit_e7):
                down_limit_e7 = history_time_e7
            if (history_time_e7 > target_abs_time_e7) and (history_time_e7 < up_limit_e7):
                up_limit_e7 = history_time_e7

        if (down_limit_e7 == 0) or (len(self._h_list) < 2) or (down_limit_e7 == up_limit_e7):
            raise DistantHistoryError(MJRuntimeError.PROCESSOR_ERROR, "DISTANT HISTORY")

        down_limit_payload, up_limit_payload = self._h_list[down_limit_e7], self._h_list[up_limit_e7]
        result_payload = {}
        delta_e7, ext_e7 = up_limit_e7 - down_limit_e7, target_abs_time_e7 - down_limit_e7
        for payload_key in down_limit_payload.keys():
            value = down_limit_payload[payload_key] + (
                    up_limit_payload[payload_key] - down_limit_payload[payload_key]) * float(ext_e7 / delta_e7)
            result_payload.setdefault(payload_key, value)

        return DataPack(payload=result_payload, pack_time=float(target_abs_time_e7 * 1e-7))


ALL_INTERPOLATIONS = {
    "liner": LinerInterpolation
}
